/*
 * Copyright 2018-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Authors: Ali Boyali, Simon Thompson
 */

#include <cmath>
#include <complex>
#include <iostream>
#include <string>
#include <vector>

#include <ros/ros.h>
#include <gtest/gtest.h>

#include "amathutils_lib/butterworth_filter.hpp"

class TestSuite :
  public ::testing::Test
{
public:
  TestSuite()
  {}
};

TEST_F(TestSuite, OrderTest)
{
  double Wp, Ws, Ap, As;
  double err_lim = 0.0001;
  Wp = 2.0;   // passband frequency [rad/sec]
  Ws = 3.0;   // stopband frequency [rad/sec]
  Ap = 6.0;   // passband ripple mag or loss [dB]
  As = 20.0;  // stop band rippe attenuation [dB]

  ButterworthFilter bf;

  bf.Buttord(Wp, Ws, Ap, As);
  Order_Cutoff NWn = bf.getOrderCutOff();

  ASSERT_EQ(5, NWn.N);
  ASSERT_NEAR(1.89478, NWn.Wc, 0.1);

  // test transfer functions
  bf.computeContinuousTimeTF();
  bf.computeDiscreteTimeTF();

  std::vector<double> An = bf.getAn();
  std::vector<double> Bn = bf.getBn();

  /*
   * Bd = [0.1913    0.9564    1.9128    1.9128    0.9564    0.1913]
   * Ad = [1.0000    1.8849    1.8881    1.0137    0.2976    0.0365]
   *
   */

  ASSERT_NEAR(1.8849, An[1], err_lim);
  ASSERT_NEAR(1.8881, An[2], err_lim);
  ASSERT_NEAR(1.0137, An[3], err_lim);
  ASSERT_NEAR(0.29762, An[4], err_lim);
  ASSERT_NEAR(0.0365, An[5], err_lim);

  ASSERT_NEAR(0.9564, Bn[1], err_lim);
  ASSERT_NEAR(1.9128, Bn[2], err_lim);
  ASSERT_NEAR(1.9128, Bn[3], err_lim);
  ASSERT_NEAR(0.9564, Bn[4], err_lim);
  ASSERT_NEAR(0.1913, Bn[5], err_lim);

  // Test with defined sampling frequency
  bool use_sampling_frequency = true;
  bf.setOrder(2);
  bf.setCuttoffFrequency(10, 100);
  bf.computeContinuousTimeTF(use_sampling_frequency);
  bf.computeDiscreteTimeTF(use_sampling_frequency);

  An = bf.getAn();
  Bn = bf.getBn();

  ASSERT_NEAR(1.0, An[0], err_lim);
  ASSERT_NEAR(-1.14298, An[1], err_lim);
  ASSERT_NEAR(0.4128, An[2], err_lim);

  ASSERT_NEAR(0.067455, Bn[0], err_lim);
  ASSERT_NEAR(0.134911, Bn[1], err_lim);

  ASSERT_NEAR(0.067455, Bn[2], err_lim);

  // bool use_sampling_frequency = true;
  // Test with order 5

  bf.setOrder(5);
  bf.setCuttoffFrequency(5, 100);
  bf.computeContinuousTimeTF(use_sampling_frequency);
  bf.computeDiscreteTimeTF(use_sampling_frequency);

  An = bf.getAn();
  Bn = bf.getBn();

  ASSERT_NEAR(1.0, An[0], err_lim);
  ASSERT_NEAR(-3.98454, An[1], err_lim);
  ASSERT_NEAR(6.43487, An[2], err_lim);
  ASSERT_NEAR(-5.25362, An[3], err_lim);
  ASSERT_NEAR(2.16513, An[4], err_lim);
  ASSERT_NEAR(-0.35993, An[5], err_lim);

  ASSERT_NEAR(5.9796e-05, Bn[0], err_lim);
  ASSERT_NEAR(2.9898e-04, Bn[1], err_lim);
  ASSERT_NEAR(5.9796e-04, Bn[2], err_lim);
  ASSERT_NEAR(5.9796e-04, Bn[3], err_lim);
  ASSERT_NEAR(2.9898e-04, Bn[4], err_lim);
  ASSERT_NEAR(5.9796e-05, Bn[5], err_lim);
}

TEST_F(TestSuite, FilterTest)
{
  std::string noisy_data_str = "-0.1371005808542652 -0.3709432297059492 -0.3451507653422276 -0.9113837979184807 -0.6859465438088277 -1.130803977436368 -0.9436363618772066 -1.112200658662783 -0.7154709912539922 -0.628148023791735 -0.5282094726504378 -0.2157311690040078 -0.003056715799466224 0.3662332996766925 0.670284443627125 0.6487756443896476 0.8346368214115352 0.9149826841939783 1.036146653313645 0.9460126068389881 0.7943776776358672 0.6102342464348015 0.3066452312825719 0.198733461388589 -0.1602110234895633 -0.2406586131034821 -0.4268010870094675 -0.7987396363590219 -1.033385865255775 -1.055610786488985 -1.062757382508302 -0.9633911470286478 -0.6443418252958325 -0.7124675186143772 -0.4080025567122524 0.04252065686037088 0.2075318314404117 0.4818736896736595 0.6794600416480069 0.8109413436334337 1.03567284578397 1.117653438563471 1.133602854790763 1.006644229888911 0.6451847596770726 0.4783734780966611 0.1718252873280794 0.2035321575192375 -0.2365538970105183 -0.3692558251997717 -0.4739912900414277 -0.6888194604625204 -0.9201177390478115 -1.161989509610712 -1.100407991794637 -0.736905330798699 -0.6824755280897932 -0.358874554258565 -0.1665457916221496 0.1001605202302536 0.1309677172745332 0.3825332805692009 0.8384843843905719 0.9532211860170634 0.8295673423157065 0.8704830233348121 1.0421278614007 0.9415846857428267 0.863842804087283 0.5271023516302398 0.1688652968072815 -0.005546215398511188 -0.4405154046643793 -0.3871913100443919 -0.7596244593073413 -1.050314577452946 -0.8843182339793805 -1.06076262276747 -1.084844273909959 -0.9643608828084225 -0.5150342619911381 -0.5265395758832851 -0.3781195868538755 0.2413643475573005 0.3337588560449873 0.375111219372476 0.6167995986238088 0.7204879493487161 0.9678822689250257 1.120159661779572 0.7709765457767805 1.001621861423223 0.7307444509197082 0.343548426954505 0.1828021589239494 -0.2295619264720048 -0.2781911866431439 -0.625440539145193 -0.8980687231868024 -0.7827750682363231 -1.178552589390432 -0.9093329052735961 -1.049481023835633 -1.001182474987026 -0.4426052073400826 -0.4118661799194657 -0.2317011749950612 0.09025343576715453 0.2698007115302677 0.7871447555968187 0.6355329513622456 0.9822069903033573 0.9345692073092348 0.8435580999650101 1.010460052500257 0.5793227220658091 0.4635456634389983 0.2809137105835985 -0.1041131309548347 -0.01176984679462592 -0.3655803148733637 -0.4580925100515778 -0.6592065315050184 -0.9034769644872731 -1.179299107432306 -0.7880616600282311 -0.7996403172704826 -0.7205184085374469 -0.45547836487063 -0.2977559382835019 -0.1309826056646411 0.07310209133731735 0.288623133164537 0.8287661453922663 1.015004167620195 1.063978546693424 0.9805513672134679 0.9263897997506001 0.8847408431863337 0.8256807467446318 0.3253744736144354 0.3563941498176002 -0.01066801936638005 -0.4102939408412518 -0.485327520266525 -0.7449985451861397 -0.9199200901414391 -0.9814755267722944 -0.8009518999327396 -0.792624049994207 -0.9800529077936975 -0.5272848135843649 -0.4680895881951053 -0.1127915725315106 -0.03027587960489609 0.3550849408910264 0.7023980985453733 0.9042168830595656 0.9945419249758659 0.9863931844077523 0.8281983932715329 0.9179242332962795 0.8707669480297838 0.6443990907912504 0.4713737379704513 0.3588035177551953 -0.2272153402418235 -0.1908838988044834 -0.3875426194781867 -0.8558876185857398 -1.097109707983982 -0.8449136523770452 -1.046117772231036 -1.076479344504307 -0.6976332724079488 -0.5843110279114125 -0.5231146808120362 -0.2246522697555833 0.1696153682464069 0.4842383224939193 0.7105227314957293 0.736965032118687 0.8587363672056691 0.8395461915319591 1.083501485243421 0.767041134362224 0.7291711029664293 0.4675518718080606 0.3855626488805129 0.2888690887893905 -0.003416636133941719 -0.4560876334297616 -0.530516328785525 -0.8525529130915486 -1.016910752762844 -1.091599956796882 -0.8326843393435859 -0.9683306221348734 -0.6261013870933115 -0.6022311785378415 -0.2388903314658904";  // NOLINT

  std::string filtered_data_actual_str = "-0.002753441084361907 -0.01725484195781884 -0.04975400356783813 -0.1062174999431817 -0.1912128428226613 -0.2989301810510817 -0.4221489737151663 -0.5502127173606841 -0.6661400280543829 -0.7506670420039955 -0.7947828321038604 -0.7973932874802139 -0.7543449724231642 -0.6632369149529028 -0.523413931639269 -0.3443826622118596 -0.1456121498254898 0.05849731511868556 0.2590274344140979 0.4458228370014395 0.60457118703906 0.7209796179186816 0.784342915658452 0.7905358128713804 0.7419245233739661 0.6438690811394077 0.5078011913072592 0.3417225935293168 0.1463478417667104 -0.06946201422463558 -0.2867899189516319 -0.4863705217250148 -0.6482805909492491 -0.7595812255558665 -0.8196963772146335 -0.8222451909388749 -0.7601442845980215 -0.6403832719697302 -0.4749610839418473 -0.2774562751848083 -0.06147873542408955 0.1623098583288291 0.3812569463197684 0.5792474751661663 0.7358537002253744 0.8329187843517778 0.8638833859395074 0.8349404576919917 0.7561794615161928 0.6320895754695137 0.472624453952305 0.2920953008067464 0.09718159661786269 -0.1097621364424631 -0.3209205225779305 -0.5129026127976234 -0.6602327587304534 -0.7511041500705388 -0.7805118830466025 -0.7485568044784834 -0.6646196126778157 -0.542440509902709 -0.3856696578075186 -0.1936366297149061 0.01686742078700648 0.2204666257417112 0.4058888381799555 0.5704544914936845 0.7062710265327672 0.8008338936406366 0.8390604072810982 0.8134297825594364 0.7259674894726467 0.5859723762528095 0.409458048435324 0.2039755424657561 -0.01940064563244734 -0.2390224942835023 -0.4428305777449027 -0.6222150359794967 -0.7581450158487467 -0.8350490451452028 -0.8563760904909364 -0.8221736736224059 -0.7253859391011803 -0.5792513063359108 -0.40483696746661 -0.2136759654720214 -0.01314384172700706 0.1923667002891544 0.3886330086369225 0.5568685938319516 0.6904230507404202 0.7769814955364819 0.8022245814128699 0.7636005929583349 0.6663498742509556 0.5221024260862979 0.3389016480645037 0.1298262107779508 -0.08784109215811892 -0.3017076006043923 -0.4959053424457757 -0.6611403706593264 -0.7841827556495551 -0.8462568397616397 -0.8481710668722843 -0.7970279860950412 -0.6958089225309999 -0.546537059356502 -0.3570969274243752 -0.1458505557933878 0.07233428939792189 0.2806627093481237 0.4646735597863049 0.6145234788052054 0.7141356518059836 0.756548639516317 0.7414761032687529 0.6734673968754936 0.5658421592734542 0.4272399041674832 0.2650442264165674 0.08590478965820197 -0.1091006899631562 -0.306743696818966 -0.4802581847680378 -0.6153774683529787 -0.7067489025824831 -0.7473199791378606 -0.7370434929247721 -0.6810163455035136 -0.584272623054924 -0.4455836478769629 -0.2613699326045031 -0.04344534830134916 0.1826250457912295 0.3923035767621003 0.5699375158731055 0.7088028828591704 0.7983918947554687 0.8285216314543671 0.8019229259756029 0.7189315715271728 0.5815092502936023 0.4039624004503001 0.1994944992526493 -0.0192909803346456 -0.2320432377908689 -0.4176528986095191 -0.5707481244142196 -0.6889595439002087 -0.7596909965584708 -0.7756826311954292 -0.7381649439313006 -0.651153588662676 -0.5152782223190963 -0.3332362090505964 -0.1193135621708166 0.1053882391950277 0.3172618329346317 0.4991716717052922 0.6467305958453895 0.7557662032092406 0.8178211489893346 0.8310030064207594 0.7920153490899937 0.6976311135557269 0.5610409251188176 0.3917792224553188 0.1875549644576527 -0.03671664151195317 -0.2545846464822846 -0.4544701297089345 -0.6244168788783545 -0.7446268263279325 -0.8098918013260585 -0.8239466933370239 -0.7828933325922628 -0.6816449080671257 -0.5248237740714399 -0.3290201388219653 -0.1158923688533112 0.09626177345040375 0.2973226163264583 0.4781747353831783 0.6229651892897048 0.7198650213829796 0.7653502639768405 0.7637173276517409 0.7205956894058898 0.6315560675099344 0.4946725632822763 0.3195523443937227 0.1162461690715345 -0.103374129990069 -0.3169156411570013 -0.5032284339124055 -0.6504863504702056 -0.7493652339202174 -0.7941438568858976";  // NOLINT

  std::vector<double> noisy_data;
  std::vector<double> filtered_data_actual;

  std::istringstream fda_iss(filtered_data_actual_str);

  for (std::string s; fda_iss >> s;)
    filtered_data_actual.push_back(std::stod(s));

  std::istringstream nd_iss(noisy_data_str);

  for (std::string s; nd_iss >> s;)
    noisy_data.push_back(std::stod(s));

  ButterworthFilter bf;

  double err_lim = 0.0001;

  bf.setOrder(2);
  bf.setCuttoffFrequency(5, 100);
  bf.computeContinuousTimeTF(true);
  bf.computeDiscreteTimeTF(true);

  std::vector<double> filtered_data;
  filtered_data.resize(filtered_data_actual.size());

  bf.initializeForFiltering();
  // set initial filter history to zero - same as in octave script
  bf.filtVector(noisy_data, filtered_data, false);

  int N = filtered_data.size();

  for (int i = 0; i < N; i++)
  {
    //  std::cerr << "actual = " << filtered_data_actual[i] << "\t filtered = " << filtered_data[i] << "\n";
    ASSERT_NEAR(filtered_data_actual[i], filtered_data[i], err_lim);
  }
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::init(argc, argv, "TestNode");
    return RUN_ALL_TESTS();
}
